#include "state.h"
#include "face.h"
#include "particle.h"
#include <iostream>

State::State()
{
    t = 0.0;

    reference = Matrix(0, 0);

    position = velocity = Vector(0);
}

State::State(double _t, unsigned int _dof, const Matrix &_ref) : t(_t), dof(_dof), reference(_ref)
{
    position = velocity = Vector(dof);

    Object *s = NULL;

    for(unsigned int i = 0; i < _ref.getRow(); i++)
    {
        particleCollide.push_back(std::make_pair (false, s));
    }
}

State::State(const Vector &_p, const Vector &_v) : position(_p), velocity(_v)
{
    t = dof = 0;

    reference = Matrix();
}

State::State(double _t, unsigned int _dof, const Matrix &_ref, const Vector &_p, const Vector &_v) : t(_t), dof(_dof), reference(_ref), position(_p), velocity(_v)
{
    Object *s = NULL;

    for(unsigned int i = 0; i < _ref.getRow(); i++)
    {
        particleCollide.push_back(std::make_pair (false, s));
    }
}

State::~State()
{

}
//time
double State::getT() const
{
	return t;
}
//return the degree of freedom
unsigned int State::getDOF() const
{
	return dof;
}
//return vector of position
const Vector &State::getPosition() const
{
	return position;
}
//change the vector position
void State::setPosition(const Vector &_p)
{
	position = _p;
}
//return the vector of velocity
const Vector &State::getVelocity() const
{
	return velocity;
}
//change the vector of velocity
void State::setVelocity(const Vector &_v)
{
	velocity = _v;
}
//change the vector of velocity and position
void State::setPositionAndVelocity(const Vector &_p, const Vector &_v){
	position = _p;
	velocity = _v;
}

//change position and the velocity of a particle
void State::setParticlePositionAndVelocity(unsigned int _i, Vector3D _position, Vector3D _velocity)
{
    for(unsigned int j=0;j<3;j++)
    {
        int pos = reference[_i][j];
        if(pos!=-1)
        {
            position[pos] = _position[j];
            velocity[pos] = _velocity[j];
        }

    }
}

//reference matrix of state
const Matrix &State::getReferenceMatrix() const
{
    return reference;
}
//change the reference matrix of state
void State::setReferenceMatrix(const Matrix &_ref){
    reference = _ref;
}
//reference matriz, degree of freedom
void State::setT_DOF_Ref_Col(double _t, unsigned int _dof, const Matrix &_ref, const std::vector< std::pair<bool, Object *> > &_col)
{
    t = _t;
    dof = _dof;
    reference = _ref;
    particleCollide = _col;
}
//operator =
State& State::operator=(const State &_s)
{
    t = _s.getT();

    dof = _s.getDOF();

    reference = _s.getReferenceMatrix();

    position = _s.getPosition();

    velocity = _s.getVelocity();

    particleCollide = _s.getCollision();

	return (*this);
}
//update state
void State::update(const State &_s, double _plusT)
{
    t = _s.getT() + _plusT;
    dof = _s.getDOF();
    reference = _s.getReferenceMatrix();
    particleCollide = _s.getCollision();
}

void State::setCollision(unsigned int _i, bool _b, Object *_s)
{
    particleCollide[_i] = std::make_pair(_b, _s);
}

std::pair<bool, Object *> State::getCollision(unsigned int _i) const
{
    if(particleCollide.empty()) std::cout << "A lista de colisão está vazia!" <<std::endl;
    return particleCollide[_i];
}

std::vector< std::pair<bool, Object *> > State::getCollision() const
{
    return particleCollide;
}
//vector of position of the particle
Vector3D State::getParticlePosition(unsigned int _i, const Vector3D& _position) const
{
    Vector3D result(_position);
    for(unsigned int j=0;j<3;j++)
    {
        int pos = reference[_i][j];
        if(pos!=-1)
        {
            result[j]=position[pos];
        }

    }
    return result;
}
//vector of velocity of the particle
Vector3D State::getParticleVelocity(unsigned int _i, const Vector3D& _velocity) const
{
    Vector3D result(_velocity);
    for(unsigned int j=0;j<3;j++)
    {
        int vel = reference[_i][j];
        if(vel!=-1)
        {
            result[j]=velocity[vel];
        }

    }
    return result;


}
//vector of position and velocity of the particle
void State::getParticlePositionVelocity(unsigned int _i, const Vector3D& _position, const Vector3D& _velocity, Vector3D *_statePos, Vector3D *_stateVel) const
{
    (*_statePos) = Vector3D(_position);
    (*_stateVel) = Vector3D(_velocity);
    for(unsigned int j=0;j<3;j++)
    {
        int pos = reference[_i][j];
        if(pos!=-1)
        {
            (*_statePos)[j]=position[pos];
            (*_stateVel)[j]=velocity[pos];
        }

    }
}

Vector3D State::getParticleNormal(Particle *_p) const
{

    std::vector<Face *> faces = _p->getFaces();

    Vector3D normal = Vector3D(0.0, 0.0, 0.0);
    unsigned int size = faces.size();

    for(unsigned int j = 0; j < size; j++)
    { //procura-se as faces a que ela pertence
        Face *f = faces.at(j);

        Particle *p1 = f->getParticleI(0);
        Particle *p2 = f->getParticleI(1);
        Particle *p3 = f->getParticleI(2);

        Vector3D pos1, pos2, pos3;
        pos1 = getParticlePosition(p1->getID(), p1->getPosition());
        pos2 = getParticlePosition(p2->getID(), p2->getPosition());
        pos3 = getParticlePosition(p3->getID(), p3->getPosition());

        normal = normal + ((pos3 - pos2).crossProduct(pos1 - pos2)).normalize();
    }


    return normal.normalize();
}

void State::getParticleFixedState(unsigned int _i, bool *_x, bool *_y, bool *_z) const
{
    if(reference[_i][0] == -1) *_x = true;
    else *_x = false;

    if(reference[_i][1] == -1) *_y = true;
    else *_y = false;

    if(reference[_i][2] == -1) *_z = true;
    else *_z = false;
}

void State::setParticleState(unsigned int _i, bool _x, bool _y, bool _z, const Vector3D &_p, const Vector3D &_v)
{
//    std::cout <<"\n\nMatriz de referencia antes: " <<std::endl;
//    reference.print();
    //tudo no estado mudará
    //será rearruamda a matriz de referencia
        //tudo permanecerá igual, até antes da particula, e mudará para o final, acrescentando ou removendo DOF
    //como o DOF mudará, o mesmo acontecerá com os vetores de velocidade e de posição

    int newDof = -1;        // caso não ache nenhum, significa que começará com zero.
    bool breaked = false;
    for(int i = _i - 1; i >= 0 ; i--)
    {
        //percorrendo todas as posições desde antes da particula foi modificada até a primeira para se achar o ultimo DOF até ela
        for(int j = 2; j >= 0; j--)
        {
            if(reference[i][j] != -1)
            {
                newDof = reference[i][j];
                breaked = true;
                break;
            }
        }
        if(breaked) break;
    }

    //setando a nova matriz de referencia para aquela particula
    //true = fixa
    //false = solta

    int newDofStart = newDof;//marcara em que ponto começou a ser mudado

    newDof += 1;       //colocando para começar do proximo numero

    if(_x) reference[_i][0] = -1;
    else reference[_i][0] = newDof++;

    if(_y) reference[_i][1] = -1;
    else reference[_i][1] = newDof++;

    if(_z) reference[_i][2] = -1;
    else reference[_i][2] = newDof++;

    int newDofEnd = newDof;//marcará o ponto onde terminou de ser modificado


    for(unsigned int i = _i + 1; i < reference.getRow(); i++)
    {//percorrendo todas as posições da particula que foi modificada até o final
        for(unsigned int j = 0; j < 3; j++)
        {
            //muda o newDof somente se estiver solto
            if(reference[i][j] != -1) reference[i][j] = newDof++;
        }
    }

    Vector oldPosition(position);
    Vector oldVelocity(velocity);

    position = Vector(newDof);
    velocity = Vector(newDof);
    //rearruma os vetores posicçao e velocidade de acordo
    int i = 0;
    int j = 0;
    for(i = 0; i <= newDofStart; i++)
    {
        //coloca os valores antigos até aquele ponto
        position[i] = oldPosition[j];
        velocity[i] = oldVelocity[j];
        j += 1;
    }

    //coloca os novos valores
    if(!_x)
    {
        position[i] = _p.x;
        velocity[i] = _v.x;
    }
    if(!_y)
    {
        i += 1;
        position[i] = _p.y;
        velocity[i] = _v.y;
    }
    if(!_z)
    {
        i += 1;
        position[i] = _p.z;
        velocity[i] = _v.z;
    }


    j = oldPosition.getRow();
    for(i = newDof; i > newDofEnd; i--)
    {
        //coloca os valores antigos andando de biaxo para cima no vetor
        position[i - 1] = oldPosition[j - 1];
        velocity[i - 1] = oldVelocity[j - 1];
        j -= 1;
    }

    if(newDof != -1) dof = newDof;
    else std::cout <<"NEWDOF ainda é negativo! Algo deu errado!: " <<std::endl;

//    std::cout <<"\n\nMatriz de referencia depois: " <<std::endl;
//    reference.print();
}

//sum operator
State operator+(const State &_s1, const State &_s2)
{
    Vector v = _s1.getPosition();

	if(_s1.getPosition().getRow() != _s2.getPosition().getRow())
	{
        std::cout <<"Erro na soma de estados!"<<std::endl;
	    exit(1);
	}

	return State(_s1.getPosition() + _s2.getPosition(), _s1.getVelocity() + _s2.getVelocity());
}
//subtraction operator
State operator-(const State &_s1, const State &_s2)
{
	if(_s1.getPosition().getRow() != _s2.getPosition().getRow())
	{
        std::cout <<"Erro na subtração de estados!"<<std::endl;
        exit(1);
    }

	return State(_s1.getPosition() - _s2.getPosition(), _s1.getVelocity() - _s2.getVelocity());
}
//multiplication by a scalar
State operator*(const State &_s, const double &_scalar)
{
	return State(_s.getPosition().scalarProduct(_scalar), _s.getVelocity().scalarProduct(_scalar));
}
//multiplication operator
State operator*(const double &_scalar, const State &_s)
{
	return State(_s.getPosition().scalarProduct(_scalar), _s.getVelocity().scalarProduct(_scalar));
}
